/**
 * Besturing
 *
 * Besturing, bestuurd de linkse en rechtste motor, met feedback van de 2 muizen.
 *
 * @author Jasper Nevens <jasper@nevensit.be>
 * @co-author Youri Hemeleers <youri.hemeleers@gmail.com>
 * @date    24/02/11
 */

#include <iostream>
#include <math.h>
#include "besturing.h"

using namespace std;
 
Besturing::Besturing(void)
{
    // mouse left
    muisL=new Muis("/dev/input/mouse1");
    muisL->setVerbose(true);
    muisL->setDPI(800);
    muisL->setAngle(M_PI);
    muisL->start();
    // mouse right
    muisR=new Muis("/dev/input/mouse2");
    muisR->setVerbose(true);
    muisR->setDPI(800);
    muisR->setAngle(M_PI);
    muisR->start();
    // motors
    int compoort;
	printf("Op welke compoort zit de motorsturing? poort: ");
	scanf("%d%*c",&compoort);
    motors=new Motors(compoort,19200);
	// be sure motors had stopped
	motors->stopMotors();
	// set current location
	setLocation(0,0,0.0);
	// set position matrix
	//posMat = 	{ 	{0.5000,	-0.5000,	0.5000,		0.5000	},
    //				{0.0000,	0.5000,		-0.0000,	0.5000	},
   	//				{0.0000,	0.0125,		-0.0000,	-0.0125	}	}; 
   	posMat[0][0]=0.5;
   	posMat[0][1]=-0.5;
   	posMat[0][2]=0.5;
   	posMat[0][3]=0.5;
   	posMat[1][0]=0.0;
   	posMat[1][1]=0.5;
   	posMat[1][2]=0.0;
   	posMat[1][3]=0.5;
	posMat[2][0]=0.0;
   	posMat[2][1]=0.0125;
   	posMat[2][2]=0.0;
   	posMat[2][3]=-0.0125;
	
	// Muizen juist zetten
	int oldXL,oldXR		;
	oldXL = muisL->getX();
	oldXR = muisR->getX();
	motors->setSpeed(50,MOTOR_FORWARD,50,MOTOR_REVERSE);
	usleep(500000);
	motors->setSpeed(0,MOTOR_BRAKE,0,MOTOR_BRAKE);
	if( (muisL->getX() - oldXL) < (muisR->getX() - oldXR ))
	{
		Muis *temp = muisL;
		muisL = muisR;
		muisR = temp;
	}
	// trug rechtrijden
	motors->setSpeed(50,MOTOR_REVERSE,50,MOTOR_FORWARD);
	usleep(500000);
	motors->setSpeed(0,MOTOR_BRAKE,0,MOTOR_BRAKE);
	
}

Besturing::~Besturing()
{
	cout << "besturing object vernield "<< endl;
}

void Besturing::execute()
{
	float a[MAXMAT][MAXMAT];
	float a1=7*M_PI/4;
	float a2=5*M_PI/4;
	float R[MAXMAT][MAXMAT];
	float ta;
	int	oldRX,oldRY,oldLX,oldLY;
	int	dxl,dxr,dyl,dyr;
	
	while(1)
	{
		usleep(100000);
		// get cur position
		dxl=muisL->getX()-oldLX; oldLX=muisL->getX();
		dxr=muisR->getX()-oldRX; oldRX=muisR->getX();
		dyl=muisL->getY()-oldLY; oldLY=muisL->getY();
		dyr=muisR->getY()-oldRY; oldRY=muisR->getY();
		a[0][0] = dxr*cos(a1) - dyr*sin(a1); 
		a[1][0] = dxr*sin(a1) + dyr*cos(a1); 
		a[2][0] = dxl*cos(a2) - dyl*sin(a2); 
		a[3][0] = dxl*sin(a2) + dyl*cos(a2);
		MatrixMultiply(posMat,a,R,3,4,1);
		this->x=R[0][0];
		this->y=R[1][0];
		this->angle=R[2][0];
		// recalculate target angle
		if(x==this->x) 
			ta = (this->angle > 0 ) ? M_PI : -M_PI;
		else
			ta=atan((y-this->y)/(x-this->x));
		if(this->x < x) ta+=M_PI;
		// set moving parameters
		// move
	}
}

/** ga naar punt en draai u dan richting angle
 *
 * @param x int in milimeter
 * @param y int in milimeter
 * @param angle float in radialen
 */
void Besturing::goTo(int x,int y,float angle)
{

}

void Besturing::setLocation(int x,int y,float angle)
{
	this->x=x;
	this->y=y;
	this->angle=angle;
}

bool Besturing::isBusy()
{
	return busy;
}

bool Besturing::isReady()
{
	return ready;
}

/** Vermenigvuldig arrays
 *
 * @param A eerste matrix om te vermenigvuldigen met dimenties M x L
 * @param B tweede matrix om te vermenigvuldigen met dimenties L x N
 * @param R restulaat matrix, met dimenties M x N
 * @param M rows of matrix A
 * @param L cols of matrix A
 * @param N cols of matrix B
 */
bool Besturing::MatrixMultiply(float A[MAXMAT][MAXMAT],float B[MAXMAT][MAXMAT],float R[MAXMAT][MAXMAT],int M,int L,int N)
{	
	int row,col,i;
	float sum;
	for(row=0;row<M;row++)
	{
		for(col=0;col<N;col++)
		{
			sum=0;
			for(i=0;i<L;i++)
				sum+=A[row][i]*B[i][col];
			R[row][col]=sum;
		}
	}
	return true;
}


